import java.lang.*;
import java.util.*;

public class OneTailAStar extends java.lang.Object implements PathFinder,Runnable
{
	public final int NO_PATH=-1,NOT_FOUND=0,FOUND=1;

	protected int [] psBGT;
	protected ArrayList edge;
	protected ArrayList done;
	protected Map map;
	int stepSpeed = 100;//higher is slower
	private int maxSteps = 1000;

	GridCell goalNode;
	Thread loop;
	double distFromStart = 0;
	private boolean findFirst = false;

	public GridCell[] findPath(Map map,int[] pushBgt)
	{

		this.psBGT=pushBgt;//Pass push Buget
		this.map = map;
		this.maxSteps=map.h*map.w*2;
		//map.resetVelocity();

		GridCell.reset();
		edge = new ArrayList();
		done = new ArrayList();
		System.out.println("calculating route");
		if(GridCell.getStartCell() == null){
			System.out.println("No start point set");
			return null;
		}
		if(GridCell.getFinishCell() == null){
			System.out.println("No finish point set");
			return null;
		}
		System.out.println("START:"+map.getStartPosition());
		System.out.println("TARGET:"+map.getFinishPosition());


		/*
		 * Reset Grid Cell Velocity to Zeros.
		 */

		for (int i=0;i<done.size();i++){
			GridCell grc= (GridCell) done.get(i);
			grc.getVel().resetVel();        	

		}

		for (int i=0;i<edge.size();i++){
			GridCell grc= (GridCell) edge.get(i);
			grc.getVel().resetVel();        	

		}

		edge.clear();
		done.clear();
		
		loop = new Thread(this);
		loop.start();


		return null;
	}

	public void run(){
		
		edge.add(GridCell.getStartCell());
		int pass =0;
		boolean found = false;
		double start,diff;
		int state=NOT_FOUND;

		//int[] copyOfPushBgt= this.psBgt.clone();//copy of push budget used for planing.

		while(state==NOT_FOUND && pass<maxSteps){
			pass++;

			
			System.out.println("=========PASS:"+pass+"==========");
			//System.out.println("=========Thead:"+loop.getId()+"==========");
			start = System.currentTimeMillis();
			//System.out.println("psBgtBFstep:"+psBgt);
			state = step(psBGT);
			diff = System.currentTimeMillis()-start;
			try{
				loop.sleep(Math.max((long)(stepSpeed-diff),0));
			}
			catch(InterruptedException e){}
			// System.out.println(diff);
		}
		if(state==FOUND){
			setPath(map,this.psBGT);

		}
		else{System.out.println("No Path Found");
		loop.stop();
		}

		//



	}


	public int step(int[] copyOfPushBgt){
		int tests = 0;
		boolean found=false;
		boolean growth=false;
		GridCell finish = GridCell.getFinishCell();
		ArrayList temp = (ArrayList) edge.clone();
		for(int i=0;i<temp.size();i++){
			GridCell now = (GridCell)temp.get(i);
			GridCell next[] = map.getAdjacent(now);
			for(int j=0;j<8;j++){//Extend to 8 neighbors
				if(next[j]!=null ){
					if(next[j]==finish){found=true;}

					//Cannot go across corner.

					if (j==4&&(next[0].isTotalBlock()||next[1].isTotalBlock())) continue;
					if (j==5&&(next[1].isTotalBlock()||next[2].isTotalBlock())) continue;
					if (j==6&&(next[2].isTotalBlock()||next[3].isTotalBlock())) continue;
					if (j==7&&(next[3].isTotalBlock()||next[0].isTotalBlock())) continue;



					next[j].addToPathFromStart(now, copyOfPushBgt);
					tests++;

					if(!next[j].isTotalBlock() && !edge.contains(next[j]))          	

					{edge.add(next[j]);growth=true;}
				}
			}
			if(found){return FOUND;}
			done.add(now);

			//edge.removeElement(now);
		}
		map.repaint();
		if(!growth){return NO_PATH;}
		// System.out.println("Tests:"+tests+" Edge:"+edge.size()+" Done:"+done.size());
		return NOT_FOUND;
	}


	public void setPath(Map map, int [] imps2){
		System.out.println("Path Found");
		GridCell.setShowPath(true);
		boolean finished = false;
		GridCell next,now;
		GridCell start = this.goalNode;
		GridCell stop = GridCell.getStartCell();
		Stack cells=new Stack();
		Stack pushes=new Stack();
		now=start;

		while(!finished){
			//next=map.getLowestAdjacent(now);

			//System.out.println(now.getPosition());
			//for(int i=0;i<now.getPsNd().length;i++){        		
				//System.out.print(now.getPsNd()[i]+" ");
			//}
			//System.out.println();	
			
			pushes.push(now.getPsNd());	//Push onto Stack. 				

			next=now.getCameFrom();
			now=next;
			
			GridCell mapCell=map.getGridCell(now.getPosition());
			mapCell.setPartOfPath(true);
			mapCell.repaint();

			cells.push(now); //Push onto Stack. 				

			if(now == stop){finished = true;}
			try{
				loop.sleep(stepSpeed);
			}
			catch(InterruptedException e){}
		}

		//System.out.println(now.getPosition());

		System.out.println("******PATH&PUSH*****");
		while(!cells.isEmpty()){
			GridCell g=(GridCell) cells.pop();
			int [] p=(int[]) pushes.pop();
			System.out.print("Grid:("+(int)g.getPosition().getX()+","+(int)g.getPosition().getY()+")"+"<=== ");
			for(int i=0;i<p.length;i++){        		
				System.out.print(p[i]+" ");
			}
			System.out.println(" /[R,L,D,U]");		

		}
		System.out.println("Grid:("+(int)start.getPosition().getX()+","+(int)start.getPosition().getY()+")"+"<=== [Target]");
		System.out.println("NOTE: R=RIGHT, L=LEFT, D=DOWN, U=UP");
		
		
		
		
		System.out.println("Done!");


	}

}